#include "implement_path_planner/implement_planner.h"
#include "implement_path_planner/implement_planner_interface.h"

#include <v3_msgs/PathGenerateAction.h>

#include <thread>

namespace global_path_planner {
namespace implement {

float p2p_dist_interval = 0.2;

ImplementPlanner __implement_Planner;

static void ImplementInterface(int16_t& action, float& p2p_dist_interval,
                            std::vector<v3_msgs::TaskPath> res_paths, 
                            uint8_t& msg_type, std::string& msg){

    bool thread_Busying = false;
    // 开始记录
    if (action == v3_msgs::PathGenerateAction::START_RECORD){
        if (__implement_Planner.GetThreadBusing()){
            // res.success = true;
            msg = "Implement record thread is busying!";
        }
        else{
            __implement_Planner.ClearPath();
            __implement_Planner.SetThreadBusing(true);
            //开启多线程
            msg = "Enter multi thread!";
            // ROS_INFO("[GenHandPolygon::LoadHandPloyGen] Enter multi thread!");
            std::thread t(&ImplementPlanner::RecordManualPath, &__implement_Planner);
            t.detach();
        }
    }
    // 取消记录
    if (action == v3_msgs::PathGenerateAction::CANCEL_RECORD){
        msg = "Canceled implement record!";
        // SaveLogInfo("INFO", msg, 0);
        // ROS_INFO("[GenHandPolygon::LoadHandPloyGen]Cancel the path recording.");
        __implement_Planner.SetThreadBusing(false);
    }
    // 保存记录
    if (action == v3_msgs::PathGenerateAction::FINISHED_RECORD){
        if (!__implement_Planner.GetThreadBusing()){
            msg = "Failed to save implement record!";
            msg_type = 1;
        }else{
            __implement_Planner.SetThreadBusing(false);
        }
        // 生成路径
        v3_msgs::TaskPath task_path;
        task_path.poses = __implement_Planner.GetImplementPath();
        res_paths.emplace_back(task_path);
        msg = "Saved implement record successful!";
    }
}

}  // namespace implement
}  // namespace global_path_planner